# Docker 容器内别名配置
# 使用方法: source bashrc_docker.sh 或添加到 ~/.bashrc

# 工作空间和环境设置
alias ws='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/ && source install/setup.bash'
alias ws1='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/ && source install/setup.bash'
alias ws2='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/src/piper_ros/ && source install/setup.bash'
alias wsd='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/example/robo_orchard_deploy'
alias wsf='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/ && source install/setup.bash && cd ../src/piper_ros/ && source install/setup.bash && conda activate sem && cd ../../robo_orchard_deploy_ros2/'

# 环境激活
alias sem='conda activate sem'
alias s1='source install/setup.bash'
alias s2='cd ../robo_orchard_deploy_ros2/ && source install/setup.bash'
alias s3='cd ../src/piper_ros/ && source install/setup.bash'

# 编译相关
alias col='colcon build --symlink-install'
alias colp='colcon build --packages-select robo_orchard_deploy --merge-install'

# CAN 接口相关
alias fpt='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/src/piper_ros && bash find_all_can_port.sh'
alias spt1='sudo ip link set can_left up type can bitrate 1000000'
alias spt2='sudo ip link set can_right up type can bitrate 1000000'
alias spt='sudo ip link set can_left up type can bitrate 1000000 && sudo ip link set can_right up type can bitrate 1000000'
alias piper='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/src/piper_ros && ros2 launch piper start_two_piper.launch.py can_left_port:=can_left can_right_port:=can_right auto_enable:=true gripper_exist:=true'

# RealSense 相关
alias sn='rs-enumerate-devices | grep "Serial Number"'
md() {
    if [ -d "/RoboOrchardLab/projects/sem/robotwin/ros2_ws/src/realsense_config" ]; then
        cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/src/realsense_config && ros2 launch realsense_config multi_d435i.launch.py
    else
        ros2 launch realsense_config multi_d435i.launch.py
    fi
}

# 模型推理服务器
alias mis='cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/example/robo_orchard_deploy && python model_infer_server.py'

# 部署节点
dep() {
    cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2 && ros2 launch robo_orchard_deploy launch_dual_arm_sync_deployment_ws.launch.py config_file:=$(pwd)/example/robo_orchard_deploy/robo_orchard_deploy_example.json
}
depn() {
    cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2 && ros2 launch robo_orchard_deploy launch_dual_arm_sync_deployment.launch.py config_file:=$(pwd)/example/robo_orchard_deploy/robo_orchard_deploy_example.json
}

# 回到中性位置
alias home='for i in {1..50}; do ros2 topic pub -1 /joint_ctrl_cmd_left sensor_msgs/JointState "{name: ['\''joint1'\'','\''joint2'\'','\''joint3'\'','\''joint4'\'','\''joint5'\'','\''joint6'\'','\''joint7'\''], position: [0,0,0,0,0,0,0], velocity: [0,0,0,0,0,0,80], effort: [0,0,0,0,0,0,0.5]}" &>/dev/null & ros2 topic pub -1 /joint_ctrl_cmd_right sensor_msgs/JointState "{name: ['\''joint1'\'','\''joint2'\'','\''joint3'\'','\''joint4'\'','\''joint5'\'','\''joint6'\'','\''joint7'\''], position: [0,0,0,0,0,0,0], velocity: [0,0,0,0,0,0,80], effort: [0,0,0,0,0,0,0.5]}" &>/dev/null & sleep 0.1; done'

# 自动初始化（每次source时执行）
echo "[bashrc_docker] 开始初始化环境..."
if [ -d "/RoboOrchardLab/projects/sem/robotwin/ros2_ws" ]; then
    if ! cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/ 2>/dev/null; then
        echo "错误: 无法进入工作空间目录 /RoboOrchardLab/projects/sem/robotwin/ros2_ws/" >&2
    else
        if [ ! -d "install" ] || [ ! -f "install/setup.bash" ]; then
            echo "[bashrc_docker] 工作空间未编译，开始编译..."
            if ! colcon build --symlink-install; then
                echo "错误: 编译工作空间失败" >&2
            else
                echo "[bashrc_docker] 工作空间编译完成"
            fi
        else
            echo "[bashrc_docker] 工作空间已编译，跳过编译步骤"
        fi
    fi
    if [ -d "/RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2" ]; then
        if ! cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/ 2>/dev/null; then
            echo "错误: 无法进入部署包目录 /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/" >&2
        else
            if [ -f "install/setup.bash" ]; then
                source install/setup.bash || echo "错误: 无法source robo_orchard_deploy_ros2的setup.bash" >&2
                echo "[bashrc_docker] 已加载 robo_orchard_deploy_ros2 环境"
            else
                echo "错误: install/setup.bash 不存在" >&2
            fi
        fi
        if [ -d "/RoboOrchardLab/projects/sem/robotwin/ros2_ws/src/piper_ros" ]; then
            if ! cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/src/piper_ros/ 2>/dev/null; then
                echo "错误: 无法进入piper_ros目录" >&2
            else
                if [ -f "install/setup.bash" ]; then
                    source install/setup.bash || echo "错误: 无法source piper_ros的setup.bash" >&2
                    echo "[bashrc_docker] 已加载 piper_ros 环境"
                else
                    echo "错误: piper_ros的install/setup.bash 不存在" >&2
                fi
            fi
        else
            echo "错误: piper_ros目录不存在" >&2
        fi
        if conda activate sem 2>/dev/null; then
            echo "[bashrc_docker] 已激活 conda 环境 sem"
        else
            echo "错误: 无法激活conda环境sem" >&2
        fi
        cd /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/ 2>/dev/null || true
        echo "[bashrc_docker] 环境初始化完成，当前目录: $(pwd)"
    else
        echo "错误: 部署包目录不存在 /RoboOrchardLab/projects/sem/robotwin/ros2_ws/robo_orchard_deploy_ros2/" >&2
    fi
else
    echo "错误: 工作空间目录不存在 /RoboOrchardLab/projects/sem/robotwin/ros2_ws/" >&2
fi

